#include "PID.h"
#include <Arduino.h>

/*constrain the maximum voltage and the minimum voltage*/
#define _constrain(amt,low,high) ((amt<low)?low:(amt>high)?high:amt)

PIDController::PIDController(float _P,float _I,float _D,float _ramp,float _limit)
{
    P = _P, I = _I, D = _D;
    output_ramp = _ramp;
    limit = _limit;
    error_prev = 0.0f;
    output_prev = 0.0f;
    integral_prev = 0.0f;
    timestamp_prev = micros();
}

// return the PID output 力矩 for errors (力矩)
float PIDController::operator() (float error)
{
    unsigned long timestamp_now = micros();
    float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
    if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;


    /*PID contribution calculation begin*/
    float proportional = P * error;
    
    float integral = integral_prev + I*Ts*0.5f*(error - error_prev);
    integral = _constrain(integral, -limit, limit);

    float derivative = D * (error - error_prev) /Ts;

    float output = proportional + integral + derivative;
    output = _constrain(output,-limit,limit);
    /*PID contribution calculation end*/

    /*limit the PID changing rate begin*/ 
    if(output_ramp > 0) 
    {
        float output_rate = (output - output_prev) / Ts;
        if (output_rate >  output_ramp) output = output_prev + output_ramp * Ts;
        if (output_rate < -output_ramp) output = output_prev - output_ramp * Ts;
    }
    /*limit the PID changing rate end*/ 
    
    integral_prev = integral;
    output_prev = output;
    error_prev = error;
    timestamp_prev = timestamp_now;
    return output;
}